package android.theHead;

import java.io.IOException;
import java.util.List;

import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorManager;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.util.Log;

public class TempoListener implements SensorEventListener {
	private float tempo = 0;
	private float tempo_avg = 0;
	private Context context;
	private static final int INTERPOL_FREQ = 20;
	private static final int SAMPLE_FREQ = 10; //Hz
	private static final int NR_SAMPLES = 256;//should be a power of 2 for efficient FFT. Should be >= 16, otherwise calculation of weightedIndx fails with IndexOutOfBounds error .
	private static final int PADDING = 3;
	//Derived constants
	private static final int SAMPLE_FACTOR = INTERPOL_FREQ/SAMPLE_FREQ;
	private static final int INTERPOL_ARRAYSIZE = (int)(((double)NR_SAMPLES/SAMPLE_FREQ)*INTERPOL_FREQ);
	private static final int INTERPOL_STEP = 1000/INTERPOL_FREQ;
	private static final int BLOCKSIZE = INTERPOL_ARRAYSIZE/8; //new samples before doing another FFT. Also must be a power of 2
	
	//double[] Gauss = {0, 0, 1,0 , 0};//noblur
	//double[] Gauss = {0, 0.1065, 0.7866,0.1065, 0};//Sigma=0.5
	double[] Gauss = {0.0545, 0.2442, 0.4026, 0.2442, 0.0545};//Sigma=1
	//double[] Gauss = {0.0702, 0.1311, 0.1907, 0.2161, 0.1907, 0.1311, 0.0702};//Sigma=2
	
	private int n, nu;
	private long startTime;
	private int previousTime = 0;
	private float previousReading = 0;
	private float currentReading_x;
	private float currentReading_y;
	private float currentReading_z;
	private float currentReading;
	private int timeWindow;
	private int nrSubsequentAccurateMeasures = 0;
	
	private static Sensor accelerometer;
	private static SensorManager senmg;
	private float[] interpolatedReadings = new float[INTERPOL_ARRAYSIZE + (2*PADDING)];//padded with 3 zeros on both sides for convolution
	private float[] sampledReadings = new float[NR_SAMPLES];
	private boolean initialArray = true;
	
	private int currentSamplepoint = INTERPOL_STEP;
	private int indx = PADDING+1; 
	private int startIndx = 0 + PADDING;
	private int stopIndx = interpolatedReadings.length - 1;
	private double sensitivity = 0.2;
	
	//debug flags
	boolean useAllSensors = true;
	boolean useSquaredAccel = false;
	
	boolean writerExists = false;
	boolean mExternalStorageAvailable = false;
	boolean mExternalStorageWriteable = false;	
	boolean writeLog = true; 
	//acceleration measure methods 
	
	public TempoListener(Context context){
		this.context = context;
		senmg = (SensorManager) this.context.getSystemService(Context.SENSOR_SERVICE); 
        accelerometer = senmg.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        senmg.registerListener(this, accelerometer, 
        		SensorManager.SENSOR_DELAY_GAME);
  
        startTime = System.currentTimeMillis();
        //Initialize padding elements with zeros
        for (int i = 0; i <= PADDING; i++){
        	interpolatedReadings[i] = 0;
        }
        for (int i = interpolatedReadings.length - PADDING; i < interpolatedReadings.length; i++){
        	interpolatedReadings[i] = 0;
        }
	}
	
	public void stopListener(){
		senmg.unregisterListener(this);
	}
	
	public float getTempo(){
		return tempo;
	}
	public float getTempoAvg(){
		return tempo_avg;
	}

	public void onAccuracyChanged(Sensor sensor, int accuracy) {
		// TODO Auto-generated method stub
	}
	public void onSensorChanged(SensorEvent event) {
		int sensor_type = event.sensor.getType();

		synchronized (this) {
			if (sensor_type == Sensor.TYPE_ACCELEROMETER) {
				currentReading_x = event.values[0];
				currentReading_y = event.values[1];
				currentReading_z = event.values[2];
				
				timeWindow = (int) (System.currentTimeMillis() - startTime);
				
				while (timeWindow >= currentSamplepoint && indx < stopIndx){
		
					if (useAllSensors){
						currentReading = (float)Math.sqrt(currentReading_x * currentReading_x + 
								currentReading_y * currentReading_y + 
								currentReading_z*currentReading_z); 
					}
					else if (useSquaredAccel){
						currentReading = (float) Math.sqrt(currentReading_y * currentReading_y);
					}
					else{
						currentReading = currentReading_y;
					}
					//linear interpolation
					interpolatedReadings[indx] = (float) (previousReading + 
							(((currentSamplepoint-previousTime)/(double)(timeWindow-previousTime))* 
									(currentReading- previousReading)));

//					if (writerExists){
//						try{
//							writer_intp.append(System.currentTimeMillis()+", ");
//							writer_intp.append(currentSamplepoint+", ");
//							writer_intp.append(round.format(interpolatedReadings[indx]));
//							writer_intp.append('\n');
//							writer_intp.flush();
//						}
//						catch(IOException e)
//						{
//							e.printStackTrace();
//						} 
//			
					indx += 1;
					currentSamplepoint += INTERPOL_STEP;	

				}
				previousTime = timeWindow;
				previousReading = currentReading;

				//interpolated array is filled
				if (indx == stopIndx){
					subsample();

					//TODO check for faster Java FFT implementations or use JNI with KissFFT
					float[] transformedReadings = fftMag(sampledReadings);

					//Check for maximum value within the expected frequency range [15,40] strokes/min
					int leftIndx = (int) Math.floor(0.25*(NR_SAMPLES/SAMPLE_FREQ));
					if (leftIndx < 2){leftIndx = 2;}
					int rightIndx = (int) Math.ceil(0.666666667*(NR_SAMPLES/SAMPLE_FREQ));
					
					float max = 0;
					int maxIndx = 2;

					for (int i = leftIndx; i <= rightIndx; i++){ 
						if (transformedReadings[i] > max){
							maxIndx = i;
							max = transformedReadings[i];
						}
					}
					Log.d("tempo", "max index: " + maxIndx); 
					Log.d("tempo", "max value: " + transformedReadings[maxIndx]); 
					
					//Weight freq of max bin with magnitude of surrounding bins		
					float totalWeight = transformedReadings[maxIndx-2]+ 
						transformedReadings[maxIndx-1] + transformedReadings[maxIndx]+
						transformedReadings[maxIndx+1] + transformedReadings[maxIndx+2];
					Log.d("tempo", "tot weight: "+ totalWeight);	
					//TODO Test this threshold!
					if (totalWeight < sensitivity){
						tempo = 0;
						Log.d("tempo", "Unnacurate readings: TEMPO set to 0");
						nrSubsequentAccurateMeasures = 0;	
					}
					else if (nrSubsequentAccurateMeasures < 2){
						tempo = 0;
						Log.d("tempo", "not enough subsequent accurate readings: TEMPO set to 0");
						nrSubsequentAccurateMeasures ++;
					}
					else{
						float weightedIndx =  (maxIndx - 2) * (transformedReadings[maxIndx-2]/totalWeight) + 
							(maxIndx - 1) * (transformedReadings[maxIndx-1]/totalWeight) + 
							maxIndx * (transformedReadings[maxIndx]/totalWeight) + 
							(maxIndx + 1) * (transformedReadings[maxIndx+1]/totalWeight) + 
							(maxIndx + 2) * (transformedReadings[maxIndx+2]/totalWeight);
	
						Log.d("tempo", "weightedVal: "+ weightedIndx);
						tempo = (weightedIndx*60)/(NR_SAMPLES/SAMPLE_FREQ);
						Log.d("tempo", "ESTIMATED TEMPO: "+ tempo);
						nrSubsequentAccurateMeasures ++;
					}

					//set new start and stop indices
					if (initialArray){
						stopIndx = startIndx + BLOCKSIZE + PADDING;
						initialArray = false;
					}
					//Circular array
					else{
						startIndx = ((stopIndx - (2* PADDING)) % INTERPOL_ARRAYSIZE) + PADDING;
						stopIndx = startIndx + BLOCKSIZE + PADDING;
					}
					indx = startIndx + PADDING;

					if ( startIndx == PADDING){
						//copy last elements of array to begin of array
						for (int i = 0;i < indx; i++){
							interpolatedReadings[i] = interpolatedReadings[INTERPOL_ARRAYSIZE - 1 + i];
						}
					}				
					Log.d("new indices", "startIndx: " + startIndx + " stopIndx: " + stopIndx);
				}

			}
		}
	}

	public void subsample(){
		//Circular array
		int j = (startIndx - PADDING) / SAMPLE_FACTOR;
		//blur only at the sample points 
		for (int i = startIndx; i < (stopIndx - PADDING); i += SAMPLE_FACTOR){
			sampledReadings[j % NR_SAMPLES] =  (float) (Gauss[0]* interpolatedReadings[i-2]+ 
					Gauss[1]* interpolatedReadings[i-1]+ Gauss[2]* interpolatedReadings[i]+ 
					Gauss[3]* interpolatedReadings[i+1]+ Gauss[4]* interpolatedReadings[i+2]);
			j++;
		}	
		Log.d("subsample","subsampling complete");

	}

	private int bitrev(int j) {

		int j2;
		int j1 = j;
		int k = 0;
		for (int i = 1; i <= nu; i++) {
			j2 = j1/2;
			k  = 2*k + j1 - 2*j2;
			j1 = j2;
		}
		return k;
	}

	public final float[] fftMag(float[] x) {
		// assume n is a power of 2
		n = x.length;
		nu = (int)(Math.log(n)/Math.log(2));
		int n2 = n/2;
		int nu1 = nu - 1;
		float[] xre = new float[n];
		float[] xim = new float[n];
		float[] mag = new float[n2];
		float tr, ti, p, arg, c, s;
		int offset = (startIndx-PADDING)/SAMPLE_FACTOR; //ADDED 

		for (int i = 0; i < n; i++) {
			//xre[i] = x[i];
			//CHANGED adapted for circular array x:
			xre[i] = x[(i + offset)% n];
			xim[i] = 0.0f;
		}
		int k = 0;

		for (int l = 1; l <= nu; l++) {
			while (k < n) {
				for (int i = 1; i <= n2; i++) {
					p = bitrev (k >> nu1);
					arg = 2 * (float) Math.PI * p / n;
					c = (float) Math.cos (arg);
					s = (float) Math.sin (arg);
					tr = xre[k+n2]*c + xim[k+n2]*s;
					ti = xim[k+n2]*c - xre[k+n2]*s;
					xre[k+n2] = xre[k] - tr;
					xim[k+n2] = xim[k] - ti;
					xre[k] += tr;
					xim[k] += ti;
					k++;
				}
				k += n2;
			}
			k = 0;
			nu1--;
			n2 = n2/2;
		}
		k = 0;
		int r;
		while (k < n) {
			r = bitrev (k);
			if (r > k) {
				tr = xre[k];
				ti = xim[k];
				xre[k] = xre[r];
				xim[k] = xim[r];
				xre[r] = tr;
				xim[r] = ti;
			}
			k++;
		}

		mag[0] = (float) (Math.sqrt(xre[0]*xre[0] + xim[0]*xim[0]))/n;
		for (int i = 1; i < n/2; i++)
			mag[i]= 2 * (float) (Math.sqrt(xre[i]*xre[i] + xim[i]*xim[i]))/n;
		return mag;
	}






}
